/**
 * @file genpid.c
 * @author hpy (yuan_hp@qq.com)
 * @brief GenPID控制实现
 * @version 0.1
 * @date 2023-07-27
 * 
 * @copyright Copyright (c) 2023
 * 
 */

#include "GenPID.h"
// #include <stdio.h>
// #include <stdint.h>

#define NULL (void *)0

/**
 * @brief 注册温控输出函数
 * 
 * @param outFunc 注册的函数实现 ,函数原型 int func(int)
 * @return int 
 * 0 ： 操作成功
 */
int GenPID_InstallOutDriver(genpid_t *g  ,OutputDriverFunction_t outFunc) {
    g->GenPIDDriverFunction  = outFunc ;
    return 0 ;
}

/**
 * @brief 初始化GenPID
 * 
 * @param Kp 设置参数P
 * @param Ki 设置参数I
 * @param Kd 设置参数D
 * @param expect 增量式PID调节的目标值
 * @return int 
 * 0 : 操作成功
 */
int GenPID_Init(genpid_t *g  ,int Kp , int Ki ,int Kd , int expect) {
    g->Kp = Kp ; 
    g->Ki = Ki ; 
    g->Kd = Kd ; 
    g->expect = expect ; 
    g->outputMaxVal = 0x7fffffff ;
    g->outputMinVal = 0x80000000 ;
    g->scale = 1000; 
    g->output = 0 ;
    g->current = 0 ; 
    g->GenPIDDriverFunction = NULL;
    return 0 ; 
}

/**
 * @brief 设置PID的缩放因子
 * 
 * @param val 设置值
 * @return int 
 * 0： 操作成功
 */
int GenPID_SetScaleFactor(genpid_t *g  ,int val) {
    g->scale= val ; 
    return 0 ; 
}

/**
 * @brief 设置PID目标值
 * 
 * @param exp 目标值
 * @return int 
 * 0:操作成功
 */
int GenPID_SetExpect(genpid_t *g  ,int exp) {
    g->expect = exp ; 
    return 0 ;
}

/**
 * @brief 获取PID目标值
 * 
 * @return int PID目标值
 */
int GenPID_GetExpect(genpid_t *g ) {
    return g->expect ; 
}

/**
 * @brief 设置输出的最大值和最小值
 * 
 * @param min 输出最小值
 * @param max 输出最大值
 * @return int
 * 0 ： 操作成功 
 */
int GenPID_SetOutMaxMin(genpid_t *g  ,int min , int max) {
	if(min>max) return 1 ;
    g->outputMaxVal = max ; 
    g->outputMinVal = min ; 
    return 0 ; 
}
#if(GENPID_SHIFT_ENABLE==1)
int GenPID_Mpu(int value , int conf) {
    int ans = 0 ;
    int n = 10 ;
    int c = 0;
    if(conf <0) {
        GENPID_INFO("conf should >= 0 and <=1023") ;
        return  0 ;
    }

    for(int i =0;i<n;i++) {
        if( (conf>>i)&0x1) {
            ans += value>>(n-i) ;
        }
    }

    return ans ;
}
#endif 

/**
 * @brief 进行一次增量式PID运算
 * 
 * @param feedBackVal 采集得到的反馈值
 * @return int 计算得出的增量
 */
int GenPID_PidCal( genpid_t *g  ,int feedBackVal ) {
    int ret  = 0 ;
    g->current = feedBackVal ; 
    g->e0 = g->e1 ; 
    g->e1 = g->e2 ; 
    g->e2 = g->expect - g->current ; 

#if(GENPID_SHIFT_ENABLE==1)
    ret = GenPID_Mpu(g->e2 - g->e1 , g->Kp) + GenPID_Mpu(g->e2 , g->Ki) + GenPID_Mpu(g->e2 - (g->e1<<1) + g->e0 , g->Kd);
    return ret;
#else 
    ret = g->Kp * (g->e2 - g->e1) + g->Ki * g->e2 + g->Kd  * (g->e2 - 2*g->e1 + g->e0) ;
    ret = ret / g->scale ; 
    return ret ; 
#endif 
}

/**
 * @brief PID计算和输出以及后续处理都在这里实现
 * 
 * @param feedBackVal 
 * @return int 
 */
int GenPID_Update(genpid_t *g  ,int feedBackVal,int sign) {
    int incVal = GenPID_PidCal(g,feedBackVal) ;
    if(sign>0)
        g->output = g->output + incVal ;
    else 
        g->output = g->output - incVal ;

    if (g->output > g->outputMaxVal) {
        g->output = g->outputMaxVal ; 
    } 
    else if (g->output < g->outputMinVal) {
        g->output = g->outputMinVal ; 
    } 

    if ( !g->GenPIDDriverFunction ) {
        return 1 ;
    }
    g->GenPIDDriverFunction(g->output); 
    return 0;
}

/**
 * @brief 获取输出值
 * 
 * @return int 返回的输出值
 */
int GenPID_GetOutput(genpid_t *g  ){
    return g->output ; 
}

